;;==========================================================================
;; Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
;;==========================================================================

#include <bsp/bspcfg.h>

    AREA |.text|,ALIGN=2,CODE, READONLY

;
;   SetupCPU()
;
    EXPORT  SetupCPU

SetupCPU
    mov	    r0, #0xd3
    msr     cpsr_c, r0

#ifdef _wmmx
    ldr     r0, =0x0043
    mcr     p15, 0, r0, c15, c1, 0

    //CPWAIT
    mrc     p15, 0, r0, c2, c0, 0   // arbitrary read of CP15
    mov     r0, r0                  // wait for it
    sub     pc, pc, #4              // branch to next instruction

    // At this point, any previous CP15 writes are
    // guaranteed to have taken effect.

    // Clear wCon
    mov     r12, #3
    mcr     p1, 0, r12, c1, c0, 0   // tmcr    wCon, r12
#endif

    mov     pc, lr

    END
